#include "com_paranoidmonkey_ode_AndrODE.h"
#include <jni.h>
#define dSINGLE
#include <ode/ode.h>
#include <android/log.h>

//#define __VERBOSE

class JNIFunc{
private:
	JNIEnv *env;
	jclass cls;
	jmethodID mid;

public:
	JNIFunc(JNIEnv *envptr,const char *clsName,const char *methodName,const char *methodSig){
		env=(JNIEnv*)envptr;
		cls = env->FindClass(clsName);
		mid = env->GetStaticMethodID(cls, methodName, methodSig);
	}

	void call(jlong o1,jlong o2){
		if (mid != NULL) {
			env->CallStaticVoidMethod(cls, mid,(jlong)o1,(jlong)o2);
		}
	}
};

void collisionHandler(void *funcPtr, dGeomID o1, dGeomID o2) {

	if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) {
		// colliding a space with something :
		dSpaceCollide2(o1, o2, funcPtr ,collisionHandler);

		// collide all geoms internal to the space(s)
		if (dGeomIsSpace (o1))
			dSpaceCollide((dSpaceID)o1, funcPtr, collisionHandler);
		if (dGeomIsSpace (o2))
			dSpaceCollide((dSpaceID)o2, funcPtr, collisionHandler);
	} else {
		JNIFunc *func=(JNIFunc*)funcPtr;
		func->call((jlong)o1,(jlong)o2);
	}
}

JNIEXPORT void JNICALL Java_com_paranoidmonkey_ode_AndrODE_collideNative(JNIEnv *env, jclass obj, jlong space){
	dxSpace *Space=(dxSpace*)space;

	JNIFunc *func=new JNIFunc(env,"com.paranoidmonkey.ode.Space","dispatchCollision", "(JJ)V");

#ifdef __VERBOSE
	__android_log_print(ANDROID_LOG_VERBOSE,"AndrODE (native)","dSpaceCollide");
#endif
	dSpaceCollide(Space, func, collisionHandler);
#ifdef __VERBOSE
	__android_log_print(ANDROID_LOG_VERBOSE,"AndrODE (native)","dSpaceCollide Done");
#endif
}

dBodyID *callbacks=0;
int numcb=0,maxcb=0,head=0;

void resizeCallbacks(int newSize){
	maxcb=newSize;
	dBodyID *newcb=new dBodyID[maxcb];

	if (callbacks!=0){
		for (int i=0;i<numcb;i++){
			newcb[i]=callbacks[i];
		}

		delete callbacks;
	}

	callbacks=newcb;
}

void movedCallback(dBodyID body){
	callbacks[head++]=body;
	if (head>=maxcb) head=0;
}

JNIEXPORT void JNICALL Java_com_paranoidmonkey_ode_AndrODE_setCallbackNative(JNIEnv *env, jclass obj, jlong id){
	if (numcb==maxcb){
		resizeCallbacks(maxcb+4);
	}
#ifdef __VERBOSE
	__android_log_print(ANDROID_LOG_VERBOSE,"AndrODE (native)","dBodySetMovedCallback");
#endif
	dBodySetMovedCallback((dBodyID)id,movedCallback);
	numcb++;
}

JNIEXPORT jlongArray JNICALL Java_com_paranoidmonkey_ode_AndrODE_getCallbacksNative(JNIEnv *env, jclass obj){
	int i;
	jlongArray arr=env->NewLongArray(maxcb);
	jlong *cb=new jlong[maxcb];

	for(i=0;i<maxcb;i++){
		cb[i]=(jlong)callbacks[i];
		callbacks[i]=0;
	}

	env->SetLongArrayRegion(arr,0,maxcb,cb);
	head=0;
	delete cb;
	return arr;
}
